/**
 * @brief Functions for dispatching a message to the correct function.
 *
 * @file dispatcher.c
 *
 * @author Robert Breznak, Peter Kalauskas
 */

#include "dispatcher.h"
#include "robot.h"
#include <stdint.h>

/**
 * The index of the device
 */
uint8_t device_index = 0;

/**
 * An array of device function used to locate and run the correct function
 * upon message receival.
 */
struct method {
	/**
	 * The device type.
	 */
	uint8_t device_type;
	/**
	 * The ID of the function.
	 */
	uint8_t function_id;
	/**
	 * A pointer to the function.
	 */
	void(*act)(uint8_t id, uint8_t *data, uint8_t length);
} device_functions[MAX_DEVICE_FUNCTIONS];

/**
 * Initializes the dispatcher by wiping the device function array and setting the
 * index to 0.
 *
 * @author Robert Breznak, Peter Kalauskas
 */
void init_dispatcher(void) {
	device_index = 0;

	int i;
	for (i = 0; i < MAX_DEVICE_FUNCTIONS; i++) {
		device_functions[i].device_type = 0;
		device_functions[i].function_id = 0;
		device_functions[i].act = NULL;
	}
}

/**
 * Registers a given action for a device type and function ID.
 *
 * @author Robert Breznak, Peter Kalauskas
 */
void register_action(uint8_t device_type, uint8_t function_id, void(*function)(
		uint8_t id, uint8_t *data, uint8_t length)) {

	if (device_index >= 0 && device_index < MAX_DEVICE_FUNCTIONS) {
		device_functions[device_index].device_type = device_type;
		device_functions[device_index].function_id = function_id;
		device_functions[device_index].act = function;

		device_index++;
	}
}

/**
 * Dispatches a message which is received by the robot.
 *
 * @param cmd an array of lenth 3 containing the device type, device id, and
 * function id.
 * @param data the data to be passed to the function.
 * @param length the length of the data being passed to the function.
 * @author Robert Breznak, Peter Kalauskas
 */
void dispatch(uint8_t *cmd, uint8_t *data, uint8_t length) {
	uint8_t device_type = cmd[0];
	uint8_t function_id = cmd[2];

	int i = 0;
	while (i < device_index) {
		if (device_functions[i].device_type == device_type
				&& device_functions[i].function_id == function_id) {
			device_functions[i].act(cmd[1], data, length);
			return;
		}
		i++;
	}
}
